基于ego

论文源码：ZJU-FAST-Lab/ego-planner-swarm: An efficient single/multi-agent trajectory planner for multicopters. (github.com)

论文原文：EGO-Swarm: A Fully Autonomous and Decentralized Quadrotor Swarm System in Cluttered Environments (arxiv.org)

提出了一种分散的、异步的多机器人自主导航系统解决方案，用于未知障碍场景下仅使用板载资源的多机器人自主导航。

文章主要：

扩展了之前的所作出的工作EGO-Planner。首先提出了一种新的、鲁棒的拓扑规划方法，几乎不需要额外的计算，避免了图3中的因局部最小值导致的动力学不可行以及导航碰撞问题。其次通过在目标函数中加入群碰撞的加权惩罚，实现了去中心化的互相避撞。

我们提出了一种分散的、异步的且对不可靠的通信和定位漂移不敏感的无人机编队结构。

将提出的方法集成到一个全自主四旋翼系统中，并发布了硬件和软件。

基于梯度的运动规划是四旋翼飞行器局部规划的主流。

采用拓扑规划来避免局部极小值，进一步加速拓扑规划的前端。

现有的无人机编队算法是通过仿真验证的，没有集成传感、映射和规划能力，且无法在野外环境中实现完全自主地运动。

ego-planner的详细方案见基于fast-planner改进的算法(1)-ego_planner阅读笔记 | Cao Dong (caodong-street.github.io) ，ego-planner主要是将轨迹生成问题作为一个非线性优化问题进行处理，权衡平滑性惩罚项$J_s$、碰撞惩罚项$J_c$、动力学可行性惩罚项$J_d$和终端进展惩罚项$J_t$(全称为terminal progress，我在ego-planer未完全看出其体现)。

但本文重新定义了描述惩罚项的函数类型：最小误差型和软单边约束型

最小误差型惩罚项包括平滑性惩罚项$J_s$和终端进展惩罚项$J_t$

最小误差型惩罚项是指使决策变量的线性变换L(Q)与期望值D之间的总误差最小的惩罚项，其描述公式为：

软单边约束型惩罚项包括碰撞惩罚项$J_c$和动力学可行性惩罚项$J_d$

软单边约束型惩罚项是用于惩罚超过特定阈值的决策变量的惩罚项$\mathcal{T}$，其描述公式为：

定义1：

均匀的可见性不同形(uniform visibility deformation,UVD)(详细见fast-planner初代论文笔记2 | Cao Dong (caodong-street.github.io))的定义：

存在两条可以被s∈[0,1]参数化的轨迹τ1(s)、 τ2(s)并且满足τ1(0) =τ2(0)， τ1(1) =τ2(1)，如果对所有情况，连接τ1(s)、τ2(s)的线段是无碰撞的。那么这两条轨迹属于均匀的可见性不同形(UVD)类。

本文通过反演v得到来${{\rm{v}}_{new}} = - {\rm{v}}$构造不同方向的距离场，再由图4(c)所示得到了新的$\left\{ {{{\rm{p}}_{new}}{\rm{,}}{{\rm{v}}_{new}}} \right\}$对。再如图4(b)所示，根据新的$\left\{ {{{\rm{p}}_{new}}{\rm{,}}{{\rm{v}}_{new}}} \right\}$对也得到了一条新的轨迹。这新旧两条轨迹之间明显是不满足定义1，从而可以说生成一条新的拓扑轨迹。采用该的轨迹生成方法没有采用直接搜索不同的拓扑路径的轨迹生成方法(详细见fast-planner初代论文笔记2 | Cao Dong (caodong-street.github.io))，而是通过之前生成的{p,v}对信息得到新拓扑轨迹，故称为隐式拓扑轨迹生成方法。

首先，我们给出无人机避障的主要思路：如图6所示，单无人机自身通过比较在相同轨迹时间内收到的其周围无人机的轨迹的距离得到一条轨迹，从而实现避撞。

此时我们对处于轨迹规划中的单无人机k构造编队碰撞惩罚项${J_{w,k}}$，软单边约束型构造编队碰撞惩罚项，其描述如下(dt代表微分)

其他不处于轨迹规划中的单无人机对应下标系数为i，$\Phi (t)$表示无人机在轨迹时间t的位置，$t_s$与$t_e$表示全局轨迹规划的开始时间和终止时间，${d_{k,t}}(t)$为定义的无人机k与无人机i之间的距离，$C{\rm{ + }}\epsilon $为无人机k与无人机i的机体安全距离之和，E=diag(1,1,1/c)表示一个对角矩阵（即指除了主对角线外的元素均为零的方阵），c>1将欧氏距离转换为z轴主轴较短的椭球型距离，以减轻下冲风险。

上述下滑线内容较抽象，我们推一下${d_{k,t}}(t)$的计算公式就可以明白这句话的意思了。

如下推导可得，最后的的{d_{k,t}}(t)的计算公式中z对应的系数为1/c，说明降低了z对应的惩罚权重，减少z轴的位移，以减轻下冲风险（障碍物里面不包括地面或者天花板，以免撞到地面或者天花板）。

椭球型距离是指$\sqrt {{x^2} + {y^2} + {{{z^2}} \over c}} $的计算项。

将加权的编队碰撞惩罚项${J_{w,k}}$与单个无人机的进行ego-planner对应的惩罚项${J_{EGO}}$得到每个单无人机的总优化问题：

本文利用均匀b样条参数化轨迹(详细参考Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight | IEEE Journals & Magazine | IEEE Xplore，B样条曲线绘制项目)

由于个体在未知环境下进行定位(没有可靠的高频回路闭合)，所以定位漂移会在飞行过程中积累。Decentralized Visual-Inertial-UWB Fusion for Relative State Estimation of Aerial Swarm | IEEE Conference Publication | IEEE Xplore（港科大组）提出了一种附加超宽带距离测量的空中群状态估计方法，实现了精确的协同定位。然而，我们更多地关注于遍历障碍环境，必须为其他应用程序保留计算和通信资源。因此，受Decentralized Visual-Inertial-UWB Fusion for Relative State Estimation of Aerial Swarm | IEEE Conference Publication | IEEE Xplore（港科大组）的启发，通过比较接收到的无人机轨迹估计的预测位置和记录无人机深度图像的测量位置，提出了一种简化的、轻量级的相对漂移估计方法。当轨迹跟踪误差可以忽略不计，且任意两个无人机中至少有一个可能会看到另一个时，这种策略就会起作用。因此，我们使用Minimum snap trajectory generation and control for quadrotors | IEEE Conference Publication | IEEE Xplore的控制器进行精确跟踪，并使用广角摄像机减少丢失无人机的可能性。(机器视觉的知识)

(这部分真的读不懂，关于无人机的slam知识了)

在利用VIO估计单无人机i的当前位置$\Phi ({t_{now}})$后，由$\Phi ({t_{now}})$为球心，以R为半径(R是一个经验参数，表示实验估计的典型漂移的上界,这个与机体安全距离相关的)确定出球形信任区域S。然后将S映射到当前捕获的深度图像，即区域${S^\prime }$，映射关系如下：

和是相机的内在矩阵和外在矩阵。z是沿主光轴与光中心的偏差。${S^\prime }$是一个需要经过复杂计算才能得到的椭圆圆锥截面。因此，我们采用近似的轴向椭圆${\overline{S} ^\prime}$来代替精确的${S^\prime }$(这里是不是能直接映射中心，然后画圆，减小算法量)。由于信任区域只是一个经验区域，因此没有必要对其进行精确的定义。

然后我们将${\overline{S} ^\prime}$内的每一个点投射到世界框架中，收集属于S的点，就得到一个点簇$\mathcal{P}$⊂S。然后将无人机观测的位置P视为$\mathcal{P}$的中心(第一个原始矩)，即

上式认为如果$\mathcal{P}$只包含相应的无人机的观察，没有任何不相关的对象，这是不能保证的。然而，由于每个无人机计划的轨迹都与附近的物体有一定的距离，所以上式在大多数情况下都适用。为了提高无人机检测的鲁棒性，还增加了像素数、$\mathcal{P}$的第二个中心矩、当前测量值与前一次测量值的偏差等判据。更严格的标准增加了假阴性率，但由于定位漂移变化缓慢，假阴性率是无害的。最后，误差将$\Phi ({t_{now}})$和P输入给一个滤波器，然后从滤波器获得估计的漂移。

简单来说，VIO漂移是在从接收轨迹估计的预测位置和信任区域内深度图像中的观测位置比较估计得到的。

我们使用占用网格图来存储静态障碍物，并使用深度图像进行地图融合。 移动无人机在第四节 -A 中得到处理。 因此，记录移动无人机并将其视为地图构建中的障碍是没有必要的，甚至是有害的。

因为这里的障碍物是ego planner中的碰撞项针对的障碍对象，而移动无人机的问题已经再本文提出的编队碰撞惩罚项中处理了，所以移动无人机不能视作地图构建中的障碍。

为了消除移动物体的影响，我们从深度图像中屏蔽并移除在第四节-B 中检测到的无人机的像素，如图 7 所示。 除此之外，覆盖大部分视图的移动物体会干扰视觉惯性里程计（visual-inertial odometry，VIO， VIO学习总结）。 因此，灰度图像上的无人机也被删除，使用相应的深度图像的相同掩码。 这里使用的无人机检测标准不太严格，因为误报比漏报更有害。

系统架构如图8所示，其中包含了单无人机和无人机编队通信系统的详细架构。

单无人机系统(包括硬件和软件设置)基于我们之前的工作EGO-Planner构造的，但包括有一个额外的模块，用于补偿VIO漂移并删除图像上的目击到的其他无人机。所有组件集成到一个自组装的250毫米轴距四旋翼。对于未知环境下的轨迹生成，采用局部规划方法。当当前轨迹与新发现的障碍物发生碰撞，或无人机接近当前轨迹的末端时(后者这里的末端没懂)，启动规划。

两个网络将系统连接起来，一个是共享轨迹的广播网络，另一个是同步时间戳和管理顺序启动的链网络。

当一个无人机生成一条新的无碰撞轨迹时，立即广播给所有无人机。然后其他无人机接收并存储这些轨迹，以便在必要时为自己生成安全的轨迹。这种闭环策略在连接稳定且延迟可以忽略的理想情况下可以正常工作。然而，这在实践中并不能保证。因此，我们提出了两种方法来减少碰撞的可能性。

首先，在网络容量下，以给定的频率广播一条轨迹。这不会造成计算负担，因为包含3-D路径点和其他参数的典型轨迹小于0.5KB。相比之下，现代的无线网络如蓝牙可以达到1Mbps以上的速度。其次，各智能体从广播网络接收到一条碰撞轨迹后立即进行碰撞检测，如果检测到潜在的碰撞，则生成新的无碰撞轨迹;该策略可以解决多个无人机在关闭时间内生成轨迹时由于延迟或丢包导致的不接收其他无人机轨迹的问题。

此外，还考虑了计算复杂度随着无人机数量的增加而增加的问题。在规划之前，每个无人机将其当前位置与接收到的周围无人机的轨迹进行比较，任何超出规划范围的轨迹都将被忽略。

基于连接的稳定链网络，用于时间戳同步和系统启动管理。在系统启动时，无人机按照预定义的顺序生成轨迹。每个无人机通过链网络接收到优先级更高的无人机的轨迹后，生成自己的初始轨迹。该策略避免了系统启动时由于无人机没有其他轨迹信息而同时生成轨迹而引起的混沌。

未完待续